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ABSTRACT 


Two  Extended  Kalman  filter  routines,  one  using  a one-step 
estimation/prediction  and  the  other  a sequential  approach, 
were  developed  and  compared  to  provide  real  time  estimates 
of  target  positions  on  the  three  dimensional  underwater  track- 
ing range  at  Naval  Underwater  Weapons  Engineering  Station, 
Keyport,  Washington.  Inputs  to  the  routines  were  acoustic 
pulse  transit  times  from  the  target  to  receiving  array  ele- 
ments which  are  non-linear  functions  of  the  position  coordin- 
ates. These  inputs  were  linearized  and  the  filter  gains  cal- 
culated on-line.  Simulated  runs  were  conducted  for  tracks  in 
the  area  of  one  hydrophone  array  and  for  tracks  that  transited 
through  multiple  arrays.  It  was  found  that  the  sequential 
estimate  routine  exhibited  better  performance  in  recovering 
from  transients  caused  by  random  measurement  noise  or  tax  et 
movement . 
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I . INTRODUCTION 


The  Naval  Underwater  Weapons  Engineering  Station,  Keyport, 
Washington  currently  operates  two  three-dimensional  under- 
water tracking  ranges  with  the  capability  of  acoustically 
tracking  torpedoes.  Underwater  tracking  employs  an  acoustic 
device  installed  in  the  object  to  be  tracked.  This  device 
transmits  timed  acoustic  pulses  which  are  received  by  bottom 
mounted  hydrophone  arrays  and  then  relayed  via  cable  to  a 
computer  at  the  observation  site  which  calculates  the  position 
of  the  object  for  each  pulse  and  plots  its  path. 

The  measured  data,  which  is  the  time  elapsed  from  trans- 
mission of  a pulse  until  its  receipt  at  the  hydrophone  array, 
is  corrupted  by  noise  due  to  the  combined  effects  of  environ- 
mental factors  and  the  measurement  instruments. 

These  noisy  tracks  are  later  analyzed,  and  measurements 
judged  most  inaccurate  on  the  basis  of  total  track  statistics 
are  removed  in  order  to  obtain  a smooth  representation  of  the 
track . 

As  stated  in  Reference  1,  the  computer  system  at  the 
Dabob  range  of  the  station  will  be  up-graded,  and  will  consist 
of  three  MODCOMP  IV  computers  by  DATACOM,  Inc.  A software 
conversion  project  will  take  place  with  applications  software 
being  developed  for  the  new  computers.  The  bulk  of  this 
development  will  consist  of  converting  the  current  tracking 
programs  and  other  related  programs  to  FORTRAN. 


tm 
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An  opportunity  exists  for  expanding  the  real-time  capa- 
bility of  the  system  by  applying  a Kalman  filter  routine 
which  can  take  as  an  input  the  transit  times  of  the  acoustic 
pulses,  and  produce  the  best  estimate  of  the  position  of  the 
tracked  object  at  a particular  time. 
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II.  THREE  DIMENSIONAL  RANGE  DESCRIPTION 


The  three  dimensional  range  described  in  Reference  2 is 
an  acoustic  system  capable  of  determining  the  trajectory  of 
suitably  instrumented  underwater  objects  in  the  vicinity  of 
a transducer  array  placed  at  the  bottom  of  the  bay.  The 
tracked  unit  (torpedo)  carries  a synchronous  clock  and  an 
acoustic  transducer.  A hydrophone  array  defines  a rectan- 
gular coordinate  system  to  which  measurements  are  referred. 
Positional  information  is  obtained  from  the  transit  times  of 
a periodic  pulsed  acoustic  signal  traveling  from  the  torpedo 
to  four  independent  hydrophones  located  on  each  array.  The 
geometry  of  the  hydrophones  and  the  coordinate  system  is 
illustrated  in  Figure  1.  On  each  array,  the  four  hydrophones 
RC'  RX'  anc*  RZ  are  on  ^our  adjacent  vertices  of  a cube. 
Each  hydrophone  is  separated  by  a distance  d=  30  feet  along 
the  edges.  The  origin  of  the  coordinate  system  is  at  the 
center  of  the  cube. 

The  transit  times  of  the  acoustic  pulse  from  the  tracked 
object  to  each  of  the  four  independent  hydrophones  can  be 
expressed  as  follows: 


Tc  = 1/VEL 


(X+d/2) 2 + (Y+d/2) 2 + ( Z+d/2 ) 


Tx  = 1/VEL 


(X-d/2) 2 + (Y+d/2)2  + (Z+d/2)2 


FIGURE  1:  The  Geometry  Used  In  the  Calculation 
of  Positional  Coordinates  (The  origin 
is  at  the  center  of  the  dashed  cube)  . 


.. 
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Tv  = 1/VEL  ! ( X+d/2 ) 2 + (Y-d/2) 2 + (Z+d/2)2 

i I 1 


T.  = 1/VEL  (X+d/2)2  + (Y+d/2) 2 + (Z-d/2)2  ! ** 

u 1 j 


Where  VEL  equals  the  velocity  of  propagation  of  sound  in 
water  and  C,  X,  Y and  Z are  the  four  hydrophones  on  each  array. 
It  is  essential  as  part  of  this  fundamental  calculation  to 
know  when  the  tracked  object  emits  an  acoustic  pulse  in  order 
to  measure  the  transit  times  to  the  hydrophones.  For  submerged 
objects  two  stable  crystal-controlled  clocks,  one  in  the  track- 
ed unit  and  the  other  at  the  computer  are  used.  Prior  to  a 
run  the  two  clocks  are  synchronized  by  radio. 

The  range  is  a high  frequency,  snort-baseline  facility 
with  the  acoustic  tracking  pulses  emitted  at  75  kHz . The 
range  layout  consists  of  six  in-line  hydrophone  arrays  spaced 
2000  yards  apart. 


III.  THEORY 


In  Reference  3,  a Kalman  filter  application  was  used 
assuming  a linear  system  and  filtering  on  the  corrupted  X,  Y 
and  Z positions  that  the  computer  had  already  calculated  from 
the  received  transit  times  of  the  acoustic  signals. 

A.  THE  EXTENDED  KALMAN  FILTER 

Since  the  transit  times  were  readily  available  and  are  non- 
linear functions  of  position,  these  equations  can  be  linearized 
and  Kalman  filter  theory  applied  using  the  extended  Kalman 
filter.  This  procedure  produces  a real-time  system,  filtering 
on  the  corrupted  transit  times  T , T^,  T^  and  T^,  without  the 
necessity  of  converting  these  times  to  positions. 

For  tracking,  a fifth  order  state  vector  was  chosen: 


X = 


The  target  was  assumed  to  maintain  constant  depth  and  any 

* 

velocity  in  the  Z direction  (Z)  was  considered  a random  exci- 

V 

tation . 

The  states  were  characterized  by  the  following  difference 
equation : 

X (K+l ) * <P  X (K)  + T W (K) 
and  the  noisy  measurement  equation: 
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?(K)  = M(X)  + V (K) 


where 

X (K)  is  the  N-dimensional  state  vector  at  time  K 

W (K)  is  the  M-dimensional  random  forcing  input 

at  time  K 

Z ( K)  is  the  J-dimensional  measurement  vector  at 

time  K 

V ( K)  is  the  J-dimensional  random  noise  vector  at 

time  K and  noise  is  assumed  white  and  zero- 

mean  Gaussian 

$ and  T are  constant  matrices 

M ( X)  is  a matrix  of  the  non-linear  measurement 

equations  which  are  a function  of  the  states. 

The  estimator  equations  are  given  by: 

X (K/K)  = X (K/K- 1)  + G (K) * Z (K)  - M (X) *X (K/K-l) 

and 

P (K/K ) = I - G (K) H (K)  P (K/K-l) 

— ^ 

where 

A 

X (K/K)  is  the  estimate  of  the  state  at  time  K given 
K measurements 

P (K/K)  is  the  covariance  of  estimation  error  matrix 
at  time  K 

G(K)  is  the  Kalman  filter  gain  at  time  K 
which  is  defined  as: 

G (K)  = P (K/K-l)  H(K)T|h 

r (K)  is  the  covariance  of  random  measurement  noise 
matrix 

H (K)  is  the  matrix  used  to  linearize  the  non-linear 
measurement  equations.  It  is  defined  as: 


(K) P (K/K-l) H (K) 


0 '1 


H(K)  = 


X (K/K-l ) 


The  measurement  equation  matrix  M(~)  is  expanded  in  a 
Taylor  series  and  linearized  around  the  one  step  prediction 
based  on  the  last  estimate.  Only  first  order  terms  are  kept. 
The  prediction  equations  are  given  by: 


X (K  + l/K)  = 4>X  (K/K)  + r W(K) 
P (K+l/K)  = t> P (K/K)  'J)T  + Q (K) 

where 


Q ( K)  is  the  covariance  of  random  excitation 
matrix  found  by: 

Q(K)  = r cov(w)rT 

and  COV(W)  is  the  covariance  of  the  random 
forcing  input  or  acceleration. 

B.  THE  SEQUENTIAL  EXTENDED  KALMAN  FILTER 

The  prediction  and  estimation  equations  just  mentioned 
represent  a system  that  is  characterized  by  a one-step  esti- 
mation and  prediction  for  each  set  of  new  measurements.  Some 
important  points  in  this  one  step  process  must  be  stressed: 

1.  New  estimates  are  only  available  after  the  prediction 
and  estimation  equations  are  calculated. 

2.  A major  part  of  the  time  period  foi  the  estimation 
calculation  is  spent  in  the  gain  equation  because  of 
the  inversion  of  the  (JXJ)  matrix  that  is  required, 
where  J is  the  number  of  observations. 
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3.  The  one-step  prediction  and  estimation  for  the  exten- 
ded Kalman  filter  is  based  on  a linearization  about 
the  predicted  value  over  a period  tK  to  tK+^  which 
might  be  extended. 

When  the  measurements  come  from  statistically  independent 
sources,  like  the  hydrophones  in  the  torpedo  tracking  pro- 
blem, a sequential  approach  can  be  taken  to  process  each 
arrival  time  separately. 

If  the  measurements  are  assumed  to  occur  simultaneously, 
they  can  be  processed  one  at  a time  and  the  result  of  pro- 
cessing one  measurement  component  is  used  in  the  following 
computation  to  process  the  next  measurement  component. 


IV.  PROBLEM  DEFINITION  - TORPEDO  TRACKING  WITH  THE 
EXTENDED  KALMAN  FILTER 


In  the  torpedo  tracking  problem,  the  non-linear  observa- 
tions are  the  four  independent  transit  times  from  the  tracked 
object  to  the  hydrophones,  Tc,  Tx,  Ty  and  Thus  the  non- 

linear measurement  matrix  M(X)  is  defined  as: 


"Tcn  vIl  J^Tx+d/2)  2+  (Y+d/2 ) 2+  ( Z+d/2 ) 2 j * 
Tx  vfL  [[X-d/2)2+(Y+d/2)2+(Z+d/2)2 


M (X)  = 


JTx-d/2) 2+ (Y+d/2 ) 2+ (Z+d/2)  2 


j^X+d/2)  2+(Y-d/2)  2 + (Z+d/2)  2 j 15 
[7x+d/2)2+(Y+d/2)2+(Z-d/2)2 


M ( X)  is  expanded  into  a Taylor  series  and  linearly  approximated 


M ( X)  a 


3M(?) 


X ( K/K-l ) 


X (K/K-l) 


The  linearizing  H matrix  is  evaluated  around  the  best 
information  available  at  the  time  which  is  the  prediction 
X (K/K-l) , and  is  used  in  the  calculation  of  the  gain  G(K)  and 
estimated  covariance  of  error  P(K/K)  equations. 

The  torpedo  dynamics  used  for  the  tracking  problem  are 

2 . . 
assumed  to  be  1/S  with  extimations  on  five  states  X position, 
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X velocity,  Y position,  Y velocity  and  Z position  (height  of 


torpedo  above  hydrophone  array) . The  mean  of  the  random  ex- 
citation is  assumed  to  be  zero  |~E  (W  (K)  ) =oJ  , and  this  simpli- 
fies the  estimate  prediction  equation  to: 

A A 

X (K+l/K ) = 4>  X (K/K) 


Also  the  mean  of  the  random  noise  is  assumed  to  be  zero 

|"e  (V (K)  ) =o"|  . In  forming  the  measurement  equation,  the  best 
i __i 

guess  of  v(K)  is  used  which  is  the  mean  producing: 

Z(K)  = M ( X) 

Four  measurements  are  taken  every  1.31  seconds,  which  is  one 
time  slot,  and  with  this  sampling  time  the  l/g2  plant  has 
state  transition  (PHI)  and  gamma  matrices  equal  to: 


and 


<t>  = 


r = 


.86 

1.31 

0 

0 

0 


1.31 

1. 

0 

0 

0 


0 

0 

J. 

0 

0 


0 

0 

1.31 

1. 

0 


0 0 

0 0 

.86  0 

1.31  0 

0 1.31 


0 

0 

0 

0 

1. 


A.  THE  SEQUENTIAL  APPROACH 

In  the  sequential  approach,  the  basic  Kalman  filter  equations 


have  been  modified  to  circumvent  the  matrix  inversion  in  the 
gain  equation  and  to  obtain  a more  accurate  estimate.  Calcu- 
lations are  performed  on  each  of  the  four  independent  transit 

times  in  the  following  order  T , T , T and  T for  each  1.31 

X Y a 

second  time  slot. 

A 

The  estimate  of  the  states,  X(K/K),  based  on  one  time 

A 

measurement  is  used  as  the  prediction  X(K/K-1)  for  the  calcu- 
lations on  the  next  measurement. 

In  this  manner  only  parts  of  the  linearizing  H matrix  and 
gain  matrices  are  used  in  each  calculation. 

After  the  linearizing  H matrix  is  formed  from 

_ mh 

X ( K/K- 1 ) 

/v 

evaluated  around  the  initial  states  X(l/0)  for  K=l,  the  first 
gain  column  corresponding  to  the  first  time  measurement  Tc  is 
calculated  from: 


3X 


0 . P(K/K-1)H^q« 

'iC0L  5iROw!(K/K-1|HLoWtRii 

where  i = 1 to  J,  and  J is  the  number  of  observations.  In 
the  tracking  problem  J = 4 corresponding  to  the  four  measured 
times . 

Thus,  the  first  row  of  the  H matrix  is  used  to  calculate 
the  first  column  of  the  gain  matrix  with  both  corresponding 
to  the  first  measured  time  Tc> 
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Next,  an  estimate  of  the  particular  observation  time  M(X) 
is  calculated 


M ( X)  = 


A 

Tc 

1 

VEL 

(X+d/2) 2 + (Y+d/2) 2+ ( Ztd/2) 2 

* 

A, 

T 

X 

1 

VEL 

(X-d/2) 2+ (Y+d/2) 2+(Ztd/2) 2 

H 

A 

T 

Y 

1 

VEL 

(X+d/2) 2+(Y-d/2) 2+( Ztd/2) 2 

A 

_Tz_ 

1 

VEL 

(X+d/2) 2+ (Y+d/2) 2+ (Z-d/2) 2 

Using  the  predicted  values  of  X,  Y,  Z from  X(K/K-1). 

The  difference  between  the  observed  transit  time  Z^  and 

A 

the  estimated  transit  time  T.  forms  the  residual  Z_TT,_  which 

i DIFF 

is  used  in  the  estimate  equation. 

- =<«</K-l)  + GiCOL  [2dif2 

This  equation  gives  an  estimate  of  the  states  based  on 
one  measurement. 

Next,  the  covariance  of  estimation  error  is  calculated 
based  on  one  measurement  using: 


P.  = 

l 


l ~ GiCOLHiROW 


Pi-1 
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where 


I equals  the  identity  matrix 

is  the  theoretical  covariance  of  estimation 
1 error  from  the  previous  measurement  or  if 
i=l,  the  prediction  P(K/K-1) 

After  the  first  iteration,  becomes  X(K/K-1)  and  P^ 

becomes  P(K/K-1)  for  the  second  iteration  which  calculates 

the  estimate  of  the  states  based  on  the  second  measurement 


After  four  iterations  (i=4),  X4  becomes  the  estimate  for 

-a 

the  time  slot,  X(K/K)  and  P^  becomes  the  updated  covariance 
of  error  P (K/K) . 

Then  the  predictions  for  the  next  time  slot  are  calcula- 
ted using: 

A A 

X (K+l/K)  = t>  X ( K/K ) 

and 

P (K+l/K)  = * P ( K/K)  4>T  + Q (K) 

The  entire  process  is  repeated  for  the  next  set  of 
measurements  forming  a sequential,  extended  Kalman  filter  to 
produce  real  time  estimates  of  the  torpedo  track. 

A listing  of  the  FORTRAN  program  designed  for  the  se- 
quential extended  Kalman  filter  is  contained  in  Appendix  B. 

The  program  is  in  modular  form  and  well  documented  by  comments 
for  ease  of  implementation.  All  repetitive  calculations  and 
utility  routines  are  separated  into  subroutines  and  are  listed 
in  Appendix  D. 
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B.  THE  MATRIX  INVERSION  APPROACH 

All  results  obtained  using  the  sequential,  extended  Kalman 
filter  were  compared  to  results  from  a traditional  extended 
Kalman  filter  using  the  equations  delineated  in  the  THEORY 
section.  This  includes  an  inversion  of  a 4x4  matrix  in  the 
gain  equation. 

G (K)  = P(K/K-1)H(K)T["h(K)P(K/K-1)H(K)T+R(K)J-1 

The  portion  of  the  equation  to  be  inverted  is  always  symmetric 
and  the  IBM-360  library  subroutine  SINV  was  used  to  perform 
this  operation. 

A listing  of  the  FORTRAN  program  designed  for  the  tradi- 
tional extended  Kalman  filter  is  contained  in  Appendix  C. 
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V.  TESTING  AND  SIMULATION 


Both  the  sequential  and  traditional  Kalman  filter  routines 
were  tested  first  using  deterministic  tracks  at  speeds  of  5.0 
to  25.0  knots  and  no  measurement  noise  with  a single  hydrophone 
array.  The  only  errors  allowed  in  the  first  phase  of  testing 
were  in  position  and  velocity  in  the  initialization  of  the 
filter. 

Computer  generated  tracks  were  tested  in  the  first  series 
of  straight  running,  constant  depth  and  constant  velocity 
torpedoes.  A variety  of  track  sceneries  were  used  transiting 
through  multiple  quadrants  including: 

1.  crossing  north  of  the  array 

2.  crossing  south  of  the  array 

3 . inbound  to  the  array 

4.  outbound  from  the  array 

5.  crossing  over  top  of  the  array 

All  runs  were  made  with  a variety  of  initialization  errors 
in  position  and  velocity. 

In  the  second  series  of  tests,  white,  zero-mean  Gaussian 
noise  was  added  to  corrupt  the  observed  transit  times. 

The  noise  was  added  to  the  straight  running,  constant 
depth  tracks.  Before  this  series  of  tests  could  be  conducted 
a gating  scheme  was  designed  to  protect  the  filter  from  spurious 
erroneous  time  or  positional  data. 

In  the  third  series  of  tests,  a number  of  torpedo  maneuvers 
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were  added  to  the  target  tracks.  One-third,  two-thirds  and 
one-G  turns  were  used.  These  tracks  were  tested  with  and  with- 
out noise. 

The  torpedo  velocities  were  increased  to  the  40  to  50  knot 
range  in  the  fourth  series  of  tests  for  straight  running  tracks 
with  and  without  noise  corruption.  Maneuvers  were  then  added 
to  these  nigher  velocity  tracks. 

In  the  last  series  of  tests,  the  handoff  routine  described 
at  the  end  of  this  section  was  added  to  the  filters  and  tracks 
that  traversed  through  the  areas  of  multiple  arrays  were 
tested . 

A.  THE  GATING  SCHEME 

The  operation  of  the  filter  may  be  adversely  affected  by 
large  measurement  noise.  One  error  of  a relatively  large 
magnitude  could  invalidate  the  filtered  output  for  many  sub- 
sequent time  slots.  Before  random  measurement  noise  and  ran- 
dom excitations  could  be  added  to  the  observed  times  for  test- 
ing, a form  of  protection  was  designed  to  guard  against  cata- 
strophic failure.  This  protection  is  provided  by  establishing 
limits  of  acceptability  for  each  of  the  measurements. 

Measurement  errors  can  occur  because  of  many  factors  in- 
cluding an  error  in  the  transit  time  of  the  acoustic  pulse 
primarily  due  to  the  receipt  of  multipath  signals  from  previous 
time  slots  that  have  bounced  off  the  surface,  bottom  or  dif- 
ferent density  layers,  or  large  errors  in  the  estimates  of 
position  or  velocity. 
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A three-sigma  gate  was  deisgned  using  the  covariance  of 
measurement  noise  (R)  and  the  covariance  of  estimation  error 
(P (K/K) ) . 

For  each  calculation  of  a state  estimate  (X(K/K)),  the 
largest  positional  covariance  of  error  was  used,  either  X,  Y 
or  Z,  and  converted  to  time  in  seconds  using  the  average  velo- 
city of  sound  in  water  for  Dabob  bay,  4860  ft/sec.  The  gate 
then  was  written  for  each  time  measurement  i = 1 to  4: 


GATE 


P (K/K) 


largest 


4860. 


R.  . 
li 


The  gate  expands  or  decreases  depending  on  the  confidence  level 
of  the  transit  time  and  position  estimate.  If  ZDIFF  which  is 
the  difference  between  the  actual  transit  time  received  and 
the  predicted  transit  time  to  a particular  hydrophone  exceeds 
the  gate,  the  measurement  is  considered  unacceptable  and  the 
filter  gain  is  set  to  zero  causing  the  filter  to  ignore  the 
data  and  take  the  prediction  of  the  states  as  the  estimate. 

/\  A 

X (K/K)  = X(K/K-1) 

— ^ 

For  the  sequential  extended  Kalman  filter,  because  of  the 
iterative  aspect,  a large  erroneous  time  measurement  zeros 
only  the  gain  column  for  that  particular  hydrophone  causing 
only  that  hydrophone's  data  to  be  ignored.  In  the  traditional 
one-step  filter,  an  erroneous  input  from  any  hydrophone  zeros 
the  entire  gain  matrix  causing  the  filter  to  ignore  data  for 
the  whole  time  slot. 
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B.  MULTIPLE  ARRAY  TRACKING 


Initial  tests  were  performed  on  tracks  in  the  area  of  one 
array.  In  order  to  more  closely  simulate  a typical  run  on  the 
range,  a scheme  was  designed  to  track  a target  through  multiple 
arrays . 

First,  a coordinate  system  was  defined  as  shown  in  Figure 
2.  The  center  of  the  coordinate  system  is  geographically  near 
the  entrance  to  Dabob  bay  in  the  simulation.  Array  number  6 is 
the  closest  array  to  the  coordinate  center.  Each  hydrophone 
is  a particular  array  has  an  X,  Y,  Z position.  In  the  simu- 
lation array  1 was  at  36,000  feet  from  coordinate  center  and 
array  6 was  6000  feet.  The  C hydrophone  was  assumed  to  be  the 
axis  location  of  each  array.  Then  each  X position  for  the  X 
hydrophone  in  each  array  was  X^+30,  each  Y position  for  the  Y 
hydrophone  was  Y^+30  and  each  Z position  for  the  Z hydrophone 
was  Z^+30.  These  72  positions,  an  XYZ  position  for  each  of  4 
hydrophones  in  6 arrays,  were  placed  into  a 6x12  matrix  HYDRO 
and  referenced  throughout  the  routine.  The  geometry  centered 
on  each  array  was  taken  out  of  the  problem  and  the  target 
position  was  based  on  a central  reference. 

The  non-linear  time  equation  became: 

T = 1/VEL  / (X-XQ) 2+(Y-Yq) 2+(Z-Zq) 2 
where  XQ , YQ  or  ZQ  is  the  position  of  a particular  hydrophone 
and  array  being  used.  In  the  filter  routine  X,  Y and  Z were 

A 

the  predicted  positions  X(K/K-1) , and  the  time  equation  was 
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HYDRO  — Hydrophone  Location  Matrix 


FIGURE  2 
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used  to  calculate  the  estimate  of  the  measurement  times  M(X). 

The  decision  parameter  used  to  determine  the  switching  from 

array  to  array  was  a straight  handoff.  If  the  predicted  X 

position  was  greater  than  3000  feet  from  the  array  in  use, 

then  an  index  (13)  was  incremented  and  the  next  row  of  HYDRO 

was  implemented.  This  placed  into  the  routine  the  X„Y^Z 
r 0 0 u 

positions  of  the  hydrophones  in  the  next  array.  The  handoff 
can  easily  be  utilized  in  real  range  operations,  as  the  tran- 
sit times  from  adjacent  arrays  are  present  at  the  computer  for 
a particular  time  slot. 

For  simulation,  it  was  assumed  that  in  all  the  arrays  each 
axis  pointed  in  the  same  direction.  In  range  operations,  the 
positions  of  the  particular  hydrophones  referenced  to  the 
central  coordinate  system  can  be  input  into  the  matrix  HYDRO 
to  correct  for  OFF  AXIS  discrepancies . 
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VI.  SIMULATION  RESULTS 


A.  SERIES  ONE 

This  series  of  tests  included  straight  running,  constant 
depth,  constant  velocity  tracks  with  no  noise.  Various  target 
speeds  were  tested  ranging  from  5.0  to  25.0  knots. 

The  only  induced  errors  in  this  series  of  tests  were  in 
initial  target  position  and  velocity.  Both  the  sequential 
and  traditional  filter  routines  effectively  handled  initial 
position  errors  in  the  X AND/OR  Y direction  from  0 to  25  feet 
and  initial  velocity  errors  from  0 to  10  ft/sec.  The  filter 
estimate  was  within  3 feet  and  1 ft/sec  in  a maximum  of  3 
time  slots.  In  a number  of  worst  case  tests,  initial  posi- 
tion errors  of  up  to  50  feet  and  initial  velocity  errors  of 
60  ft/sec  and  80  ft/sec  were  used.  Both  filter  routines  had 
the  estimate  on  track  within  seven  time  slots. 

Figure  3 is  a geographical  plot  of  a typical  series  one 
test  using  the  sequential  extended  Kalman  filter.  The  initiali- 
zation of  the  filter  was  14  feet  off  in  X and  21  feet  off  in 
Y with  no  error  in  Z.  For  a 25  knot  target  initial  velocity 
errors  were  3 ft/sec.  Figures  4 through  6 depict  the  devia- 
tion in  feet  between  the  estimated  and  true  positions, 

A 

X (K)  - X (K/K) , where  A = 1,  3 or  5 . 

A A 

There  was  no  great  difference  in  performance  between  the 
sequential  and  traditional  filter  routines  in  this  series  of 
tests . 
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Geographic  Plot  Straight  Running  Track 
in  the  area  of  a single  Array 
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FIGURE  5:  X Deviation  from  True  versus  Time  for  a Straight 
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B.  SERIES  TWO 


In  the  second  series  of  tests  random  white  Gaussian  noise 
was  added  to  the  transit  time  measurements  for  straight  running 
tracks.  Target  velocities  varied  from  5.0  to  25.0  knots.  Ini- 
tial position  errors  ranged  from  0 to  25  feet  and  initial  velo- 
city errors  from  0 to  10  ft/sec. 

Both  the  sequential  and  traditional  routines  performed 
well  against  the  random  noise.  Estimate  deviations  from  true 
track  in  X and  Y positions  were  not  observed  greater  than  3 
feet  after  lock-on  was  acquired.  Figures  7 and  8 depict  the 
estimate  deviation  from  true  track  for  the  sequential  routine. 
Initial  positions  were  9 feet  off  in  X,  13  feet  in  Y and  2 feet 
in  Z . 


C.  SERIES  THREE 

In  this  series  of  tests  maneuvers  were  added  to  the  torpedo 
tracks  and  the  filter  was  tested  with  and  without  noise. 

Figure  9 is  a geographical  plot  of  a 25  knot  target  in  a 
noiseless  environment  with  1/3-G  turns  at  time  slots  25  and  65. 
Figure  10  depicts  the  Z position  versus  the  time  of  run.  In 
the  initialization  of  the  filter  X was  off  13  feet  and  Y was 
off  21  feet.  Figure  11  depicts  the  deviation  by  the  estimate 
from  the  true  track  in  the  X direction,  for  the  traditional 
inversion  approach.  Figure  12  shows  the  deviation  for  the 
same  track  using  the  sequential  extended  Kalman  filter.  When 
the  target  initiated  the  turn  at  time  slots  25  and  65,  the 
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FIGURE  7:  X Deviation  from  True  versus  Time  for  a straight 
Running  Track  with  Molse  in  the  Area  of  a Single 
Array  — Sequential  Routine 
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FIGURE  12:  X Deviation  from  True  for  a Track  with  1/3-G 
Turns  without  Noise  in  the  Area  of  a Single 
Array  — Sequential  Routine 
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covariance  of  estimation  error  and  thus  the  filter  gains  in- 
creased allowing  more  weight  to  be  placed  on  the  incoming 
data  (transit  times) . In  this  manner  the  filter  was  able  to 
adjust  the  velocities  in  the  X and  Y directions  and  force  the 
estimate  back  on  track.  Results  from  both  routines  were  com- 
parable as  is  shown  in  Figures  11  and  12.  The  routine  using 
the  sequential  extended  Kalman  filter  deviated  a maximum  of 
four  feet  at  the  turn  and  was  back  on  track  within  3 time 
slots,  while  the  traditional  filter  routine  had  a slightly 
greater  deviation  and  required  2 more  time  slots  to  acquire 
lock  again.  Figure  13  shows  the  deviation  of  the  estimate 
from  true  track  in  the  Y direction  for  this  same  run  for  the 
sequential  filter  and  Figure  14  for  the  traditional  filter. 
Both  estimates  deviated  a maximum  of  3 feet  at  the  turn  and 
re-acquired  lock-on  in  4 time  slots  with  the  sequential  rou- 
tine reacting  slightly  better  to  the  turns. 

Next,  target  tracks  with  2/3  and  1-G  turns  were  tested. 
Figure  15  is  a geographic  plot  of  this  track  with  a 1-G  turn 
at  time  slot  25  and  a 2/3-G  turn  at  time  slot  65.  Figure  16 
depicts  the  Z position  vs  time  for  this  track.  With  an  ini- 
tialization error  of  13  feet  in  X and  21  feet  in  Y,  the  maxi- 
mum deviation  after  lock-on  was  again  at  the  turn  points. 
Figure  17  depicts  this  deviation  from  true  track  in  the  X 
direction,  for  the  sequential  routine  with  a maximum  deviation 
of  approximately  5.5  feet  for  the  1-G  turn.  Lock-on  was  re- 
acquired in  a maximum  of  4 time  slots.  Figure  18  shows  the 
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FIGURE  16:  Z Position  versus  Time  for  a Track  with  2/3-G 
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deviation  from  true  in  the  Y direction  with  a maximum  deviation 


after  lock-on  for  the  1-G  turn  of  under  4 feet.  Runs  with 
the  traditional  extended  Kalman  filter  routine  showed  slightly 
larger  deviations  with  more  reaction  time  required  to  re-ac- 
quire  lock-on. 

In  the  final  tests  for  this  series,  zero  mean,  white  Gau- 
ssian noise  was  added  to  corrupt  the  observed  transit  times. 
Figure  19  depicts  the  estimate's  deviation  from  true  track 
in  the  X direction  using  the  traditional  filter  routine.  The 
target  experienced  1/3-G  turns  at  time  slots  25  and  65.  Figure 

20  shows  the  same  track  in  noise  with  the  same  filter  parame- 
ters when  run  with  the  sequential  extended  Kalman  filter  rou- 
tine. Comparison  shows  that  the  latter  had  approximately  2 
feet  less  maximum  deviation  and  that  while  the  traditional 
routine  had  trouble  re-acquiring  lock-on  after  the  turn,  the 
sequential  routine  regained  track  within  4 time  slots.  Figures 

21  and  22  depict  the  same  type  of  behavior  for  deviation  in 

the  Y direction.  Maximum  deviation  again  was  slightly  less  and 
lock-on  more  efficiently  re-acquired  using  the  sequential  rou- 
tine. 

In  the  cases  involving  more  radical  maneuvers , the  se- 
quential routine  continued  to  perform  better  against  the  mo- 
deled noise.  Figure  23  shows  the  estimate's  deviation  from 
true  track  in  the  X direction  for  a run  with  a 1-G  maneuver  at 
time  slot  25  and  a 2/3-G  maneuver  at  time  65  in  noise.  This 
graph  for  the  traditional  routine  shows  maximum  deviation  at 


l.JO  2 i.  00 


51.OO  T>  - JO 


Ol.OC 


-3.2? 


-5.2b 


X-'.C'.L  = : 0.  1 25E  O'.  'J'O  I T S 

Y-,C:.l£:  0.  "’S  00  '.IM  iTO 

i.-vur;  in  r-  ■:«  mue  x 


51.00 

TIME  SLOTS 


6.  JO 


1 31 . 00 


FIGURE  19:  X Deviation  from  True  for  a Track  with  1/3-G 
Turns  with  Noise  in  the  Area  of  a Single 
Array  — Traditional  Routine 
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FIGURE  21:  Y Deviation  from  True  for  a Track  with  1/3-G 
Turns  with  Noise  in  the  Area  of  a Single 
Array  — Traditional  Routine 
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FIGURE  22:  Y Deviation  from  True  for  a Track  with  1/3-G 
Turns  with  Noise  in  the  Area  of  a Single 
Array  — Sequential  Routine 
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the  1-G  turn  of  approximately  9 feet  and  the  difficulty  the 
filter  had  in  re-acquiring  lock-on.  Figure  24,  which  is  the 
same  track  and  filter  parameters  run  with  the  sequential  rou- 
tine, shows  a marked  improvement  in  both  maximum  deviation  and 
efficiency  in  re-acquiring  the  track  after  a turn  in  noise. 

Figures  25  and  26  demonstrate  again  the  better  performance 
of  the  sequential  extended  filter  routine  regarding  deviation 
in  the  Y direction  for  tne  more  radically  maneuvering  track. 

D.  SERIES  FOUR 

In  the  fourtn  series  of  tests,  tne  target  speeds  were  in- 
creased to  the  40  to  50  Knot  range  in  order  to  bring  the  simu- 
lation in  line  with  speeds  actually  experienced  on  the  range. 
In  a noiseless  environment,  both  routines  showed  similar  per- 
formance with  a maximum  deviation  from  true  track  during  the 
run  under  2 feet.  With  random  noise  added  this  deviation  in- 
creased and  Figure  27  is  a plot  of  the  deviation  in  the  X 
direction  using  the  traditional  routine.  When  compared  to 
Figure  28  which  is  the  same  track  using  the  sequential  routine 
with  the  same  parameters,  a marked  improvement  in  maximum  de- 
viation and  lock-on  efficiency  is  noticeable.  Better  perfor- 
mance by  tne  sequential  routine  was  also  present  regarding 
deviation  in  the  Y direction. 

When  ]/3-G  turns  were  introduced,  the  routines  performed 
similarly  in  a noiseless  environment.  With  random  noise  and 
maneuvers  the  routines  were  again  comparable  with  the  tra- 
ditional routine  performing  slightly  better  as  is  shown  in 
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FIGURE  23:  X Deviation  from  True  for  a Track  with  2/3-G 
and  1-G  Turns  with  Noise  in  the  Area  of  a 
Single  Array  — Traditional  Routine 
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FIGURE  24:  X Deviation  from  True  for  a Track  with  2/3-G 
and  1-G  Turns  with  Noise  In  the  Area  of  a 
Single  Array  — Sequential  Routine 
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FIGURE  25:  Y Deviation  from  True  for  a Track  with  2/3-G 
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FIGURE  26:  Y Deviation  from  True  for  a Track  with  2/3-G 
and  1-G  Turns  with  Noise  in  the  Area  of  a 
Single  Array  — Sequential  Routine 
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FIGURE  27:  X Deviation  from  True  for  a Straight  Running 
l\2  Knot  Target  with  Noise  in  the  Area  of  a 
Single  Array  — Traditional  Routine 
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FIGURE  28:  X Deviation  from  True  for  a Straight  Running 
42  Knot  Target  with  Noise  in  the  Area  of  a 
Single  Array  — Sequential  Routine 
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Figures  29  and  30.  Figure  29  depicts  the  estimate's  deviation 
from  true  in  the  X direction  for  the  traditional  routine  and 
Figure  30  for  the  sequential.  In  this  track  the  target  man- 
euvered, using  1/3-G  turns,  at  time  slots  25  and  65.  A 
slightly  better  performance  was  also  exhibited  by  the  tradi- 
tional routine  in  the  Y direction. 

In  tests  in  which  more  radical  maneuvers  were  made,  the 
traditional  routine  again  exhibited  slightly  better  performance 
in  noise.  Figure  31,  shows  the  deviation  in  the  X direction  for 
a track  with  a 1-G  turn  at  time  25  and  a 2/3-G  turn  at  65  using 
the  sequential  routine.  Comparision  with  Figure  32,  which  is 
the  same  track  using  the  traditional  routine,  indicates  that 
the  performances  were  similar  with  the  traditional  routine 
having  a slight  edge. 

E.  SERIES  FIVE 

In  the  last  series  of  tests,  the  targets  were  tracked 
through  multiple  arrays  using  the  handoff  scheme  described  in 
the  previous  seciton.  Figure  33  is  a geographic  plot  of  a 
typical  track  including  hydrophone  positions.  Figure  34  de- 
picts the  estimates  deviation  from  true  in  the  X direction 
using  the  traditional  routine  for  a straight  running  target  in 
noise.  Figure  35  is  the  X deviation  for  the  same  track  using 
the  sequential  routine.  Through  the  multiple  arrays,  both 
routines  were  comparable,  and  the  handoff  from  array  to  array 
was  smooth  with  no  glitches.  Figures  36  and  37  also  show  com- 
parable results  for  deviation  in  the  Y direction  for  this  same 
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FIGURE  29:  X Deviation  from  True  for  a Track  with  1/3-G 
Turns  at  b2  Knots  with  Noise  in  the  Area  of  a 
Single  Array  — Traditional  Routine 
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FIGURE  30:  X Deviation  from  True  for  a Track  with  1/3-0 
Turns  at  42  Knots  with  Noise  in  the  Area  of  a 
Single  Array  — Sequential  Routine 
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FIGURE  31:  X Deviation  from  True  for  a Track  with  2/3-G 
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FIGURE  32:  X Deviation  from  True  for  a Track  with  2/3-G 
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Geographic  Plot  of  a Straight  Running 
Track  through  Multiple  Arrays 
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FIGURE  3^:  X Deviation  from  True  for  a Straight  Running 
Track  through  Multiple  Arrays  with  Noise  — 
Traditional  Routine 
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FIGURE  35:  X Deviation  from  True  for  a Straight  Running 
Track  through  Multiple  Arrays  with  Noise  — 
Sequential  Routine 
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FIGURE  36:  Y Deviation  from  True  for  a Straight  Running 
Track  through  Multiple  Arrays  with  Noise  — 
Traditional  Routine 
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FIGURE  37:  Y Deviation  from  True  for  a Straight  Running 
Track  through  Multiple  Arrays  with  Noise  -- 
Sequential  Rout ine 
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run . 


The  target  was  traveling  at  40  knots  and  handoff  was 


accomplished  at  the  following  times: 


Handoff  1 to  2 
Handoff  2 to  3 
Handoff  3 to  4 
Handoff  4 to  5 
Handoff  5 to  6 


Time  Slot  77 
Time  Slot  168 
Time  Slot  260 
Time  Slot  352 
Time  Slot  443 
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VII.  CONCLUSIONS 


Both  extended  Kalman  filter  routines  designed  will  provide 


Implementation  at  the  range  computer  facilities  can  be 


into  a file 


in  memory  and  reading  them  into  the  routine  with  subroutine 


DREAD.  The  hydrophone  positions  can  also  be  read  into  a file 


to  be  referenced  during  operation.  The  initial  covariance  of 


by  multipath  signals  or  from  noise  spikes  will  cause  the  gains 


to  zero,  making  the  estimated  position  equal  to  the  predicted 


thus  putting  the  filter  in  'coast 


in  the  linearizing  matrix  H.  This  model  is  depicted  in  Figure 


38.  With  a constant  Z,  the  targets  position  at  time  t^  is 
XqYq.  By  the  time  the  transmitted  signal  has  reached  the  hy- 
drophone array,  the  target  with  X velocity  (V^)  and  Y velocity 
(V  ) has  moved  to  the  position  at  t0 : 


where  T is  the  transit  time  of  the  signal 


MOVING  TARGET  in  the  X-Y  PLANE 


X=  X0  + VXT 
Y=  Yq  + VyT 


FIGURE  38:  Geometry  used  to  remodel  the  System 
to  include  X and  Y Velocities  in  the 
H Matrix 


T = 1/VEL  / XQ  + Yq 

and  VEL  is  the  velocity  of  propagation  of  sound  in  water. 
Including  the  new  positions  corresponding  to  the  target 
location  when  the  signal  is  received  gives  a new  equation  for 


the  transit  time. 


'(X-VXT)2  + (Y-VyT)  2 


solving  for  T produces: 


(XVx+YVy)  - / (XVx+YVy) 2- (Vx2+Vy2-VEL) (X2+Y2+Z2) 

(vx2+vy2-vel) 

Upon  testing,  this  added  complexity  did  not  give  a correspond- 
ing increase  in  the  quality  of  the  estimate. 

In  general,  both  routines  were  comparable  with  the  se- 
quential filter  having  the  following  advantages: 

1.  Less  deviation  and  quicker  lock-on  time  after  a 
modest  maneuver 

2.  With  the  three-sigma  gate  utilized  a noise  spike 
encountered  will  negate  data  from  only  that  par- 
ticular hydrophone 

3.  A matrix  inversion  is  not  necessary. 


72 


APPENDIX  A 


PROGRAM  DESCRIPTION  AND  FEATURES 

Two  programs  were  written  to  implement  the  extended  Kalman 
filter  routine  for  torpedo  tracking.  The  first,  THEFIV,  uti- 
lizes the  sequential  approach  described  in  section  IV-A,  and 
the  second  a traditional  matrix  inversion  approach  as  descri- 
bed in  section  IV-B.  Both  routines  use  the  same  utility  pro- 
grams and  are  modularized  for  ease  of  implementation. 

1.  THEFIV  - Sequential  Routine 

This  program  is  general  in  nature  and  many  of  the 
parameters  of  the  Kalman  routine  are  variable  including: 

a.  The  number  of  states  in  the  routine  - N 

b.  The  number  of  random  forcing  functions  - M 

c.  The  number  of  measurements  - J 

d.  Data  rate  or  sample  time  - TO 

and  *i>(l,2),  {>(3,4) 

e.  Number  of  time  slots  - JTIME 

The  constant  matrices  PHI,R,COVW  and  GAMMA  are  read  in  using 
subroutines  in  the  utility  program  AUX.  The  filter  is  ini- 
tialized with  P(l/0)  and  X(l/0)  (initial  covariances  of  esti- 
mation error  and  states)  also  using  AUX.  The  first  state 
estimate  is  at  time  1 and  continues  until  ITIME  = JTIME+1, 

True  measurement  times  (ZI)  are  read  in,  four  for  each  time 
slot  (T  ,T  ,T  ,T  ) , using  the  subroutine  DREAD  listed  in  AUX 
and  corrupted  by  zero-mean,  white  Gaussian  noise  using  the 


m ' 


IBM-360  subroutine  SNORM.  For  each  of  the  four  time  measure- 
ments the  corresponding  row  of  the  linearizing  H matrix  is 
calculated  in  the  utility  subroutine  CHROW,  and  the  corres- 
ponding gain  matrix  column  GI  is  found.  These  row  and  column 
values  are  then  utilized  in  forming  the  covariance  of  exti- 
nction error  for  the  particular  time  measurement  PI.  Next 

a 

the  estimate  of  the  observation  time  M(X)  from  that  particular 
hydrophone  called  ZHAT  is  formed  using  the  subroutine  CZHAT 
and  the  residual  ZDIFF  = ZI-ZHAT.  Finally,  the  estimate  of 
the  states  XI  based  on  one  time  measurement  is  calculated,  and 
the  process  is  repeated  for  the  next  measurement.  After  four 
iterations,  XI  becomes  the  state  estimate  and  PI  becomes  the 
updated  covariance  of  estimation  error  PKK,  and  the  predictions 
of  the  states  and  covariances  XKKMl  and  PKKM1  are  formed. 

For  testing,  this  program  used  the  IBM-360  library  sub- 
routine PLOTP  to  obtain  plots  of  the  states  and  covariances 
versus  time,  estimate  deviation  from  true  versus  time  and  a 
geographic  track. 

2.  THESIS  - Traditional  Routine 

THESIS  utilized  the  same  format  as  THEFIV.  The  para- 
meters N,M,JS,  sample  time  and  number  of  time  slots  were  still 
variable,  and  the  filter  initial  conditions  and  constant 
matrices  were  read  in  by  the  subroutines  located  in  AUX.  True 
time  measurements  were  again  read  in  for  each  time  slot  through 
DREAD  and  corrupted  with  noise  from  SNORM.  The  linearizing  H 
matrix  was  formed  row  by  row  using  subroutine  CHROW  and  used 

74 


-■ - - - 


to  calculate  the  gain  matrix  G.  The  symmetric  matrix  inver- 
sion in  the  gain  equation  was  done  by  the  IBM-360  library 
subroutine  SINV.  The  estimate  vector  of  the  observations 
ZHAT  is  formed  iteratively  by  CZHAT  and  used  to  calculate  the 
residual  vector  ZD IFF . Next  the  estimate  XKK  and  updated  co- 
variance  of  error  matrix  PKK  is  calculated  once  for  each  time 
slot.  Finally,  the  predictions  XKKMl  and  PKKMl  are  formed 
before  the  process  is  repeated  for  the  next  time  slot.  PLOTP 
was  again  utilized  for  the  output  graphs. 

3.  Utility  Programs 

These  subroutines  were  designed  to  be  used  for  re- 
petitive calculations  and  processes.  The  first,  AUX  performs 
all  data  input  functions  and  matrix  manipulations  including: 

a.  PROD  - multiplying  two  matrices 

b.  MMULT  - multiplying  a matrix  and  a vector 

c.  VMULT  - multiplying  two  vectors 

d.  MREAD  - reading  in  a matrix 

e.  TRANS  - transposing  a matrix 

f.  ADD  - adding  two  matrices 

g.  VREAD  - reading  in  a vector 

h.  DREAD  - reads  in  a matrix  containing  observed 

time  data 

i.  TRREAD  - reads  in  a matrix  containing  true  posi- 

tional data  for  comparison 

The  second  utility  subroutine  CZHAT  calculates  the  esti- 
mate of  the  observation  times  (TC,TX,TY  or  Tz)  using  the 

A 

predicted  state  values  (X'(K/K-1)). 
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The  subroutine  CHROW  calculates  a row  of  the  lineariz- 
ing H matrix,  each  row  corresponding  to  a particular  observa- 
tion time  measurement. 


A.  VARYING  STATE  TRANSITION  MATRIX 
2 

In  the  1/S  model  that  represents  the  dynamics  of  the 
Kalman  filter,  the  State  Transition  and  GAMMA  matrices  are  as 
follows : 


$ 


1 T 0 0 

0 10  0 
jO  1 T 0 | 

0 0 1 0 | 

jo  0 0 1 ! 


1 


1 T2/2  0 

I 

T 0 

0 T2/2 

0 T 

0 0 


0 

0 

0 

0 

T 


J 


where  T is  the  sampling  time.  In  the  tracking  problem,  this 
time  is  not  always  constant.  As  is  shown  in  Figure  39  it 
varies  with  the  transit  times  required  from  positions  in 
adjacent  time  slots.  In  Figure  39,  the  target  is  at  position 
1 at  arbitrary  time  tQ,  and  is  at  position  2,  the  next  time 
slot,  at  tg+T  where  T is  the  time  between  pings  of  the  target 
transducer  = 1.31  seconds.  The  times  that  the  transmitted 
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Moving  Target  in  the  X-Y  Plane 

Ts  = (t0  + T + t2)  - (to  + ti) 

Ts  = T + ( t 2 ~ ti) 

IGURE  39:  Geometry  used  for  the  Time  Varying 
State  Transition  Matrix 


il 


I 


signals  are  received  by  the  hydrophone  array  are: 

Time  signal  received  from  position  1 = tg+t^ 

Time  signal  received  from  position  2 = t^+T+t2 

where  t^  and  t2  are  the  transit  times  from  position  1 
and  position  2 respectively.  The  difference  between  these  two 
times  signals  are  received  is  the  sampling  time  Tg . 

TS  - <VT+t2>  - (t0+tl> 

TS  * T + 

Therefore,  the  sampling  time  differs  each  time  slot  from  the 
clocked  ping  time  by  the  difference  in  the  adjacent  slot 
transit  times. 

In  the  programs  THEFIV  and  THESIS,  this  difference 
is  calculated  using  the  average  measured  times  from  the  four 
hydrophones  for  each  time  slot. 

B . ADAPTIVE  Q 

The  Q matrix  which  appears  in  the  predicted  covariance  of 
error  equation 

P (K+l/K)  = 3>P  ( K/K)  3>T+Q  (K) 

and  is  formed  by 

q (k)  = rcovwrT 

is  a measure  of  the  amount  of  target  maneuverability  that  can 
be  handled  by  the  filter.  If  more  random  excitations  (or 
accelerations)  by  the  target  is  expected,  Q is  increased  which 
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in  turn  increases  the  covariances  of  estimation  error  P(K+1/K) 
and  the  filter  gains  G.  Corresponding  the  filter  puts  more 
emphasis  on  the  incoming  data  and  is  better  able  to  see  and 
react  to  target  turns.  However,  if  the  filter  gains  are  in- 
creased the  filter  bandwidth  is  widened,  which  lets  in  more 
noise,  and  makes  the  filter  more  susceptible  to  error.  The 
adaptive  Q routine  (Reference  4)  varies  the  Q matrix  as  the 
velocities  in  the  X and  Y directions  are  increased  or  de- 
creased. This  routine  was  implemented  in  the  subroutine  QFIND 
with  inputs  of: 

SIGACC  = expected  maximum  acceleration  in  the  X or  Y 
direction  in  ft/sec 

SIGDIV  = expected  maximum  acceleration  in  the  Z direction 
in  ft/sec 

SIGCC  = expected  maximum  target  course  change  in  degrees/ 
sec 

QFIND  is  listed  in  Appendix  D. 
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THEFIV 


oooooooooooooooooooooo 

,-4<\im»i-in>Of^®  O'0«-if\jm>l-in'0f'-cn0'o.-a'l' 
OOOOOOOOOHHHHrtHKrtHHNNN 
OOOOOOOOOOOOOOOOOOOOOO 
OOOOOOOOOOOOOOOOOOOOOO 
UJ  UJ  UJ  LL)  u 1 UJ  LL)  u.1  LU  LU UJ  UJ  LU  UJ  LU  uj  UJ IU  LL)  LU LL1 LU 


ooo 


O rH<\i 


mmm 

ooo 

ooo 


LLIUJUJ 

XXI 


ooooooooooooooooo 
sj-  in  -o  r-  « o'  o ^ <vi  co  .4-  m o i"-  oc  O'  c 
rnniforootfO'i-'Oooo-O'O'O'OvOr- 
OOOOOOOOOOOOOOOOo 
OOOOOOOOOOOOOOOOO 

LULLHULULUUJluUJLULLJIJJUJLIJIIJUJLUUJ 

'iixiixm 


•* 

•• 

O 

H 

•* 

l n » 

in 

•w 

o 

ino 

— -H 

— X 

►in 

h- mm 

— — •— CJm 
1^-min — ll 
O.X  — -mu. 

» -^inOi-< 

• > . »o 

ir.|i\QO-X- 

- -<r 

m m »tn— — ■— 
— — — ojcjuj 
^ainnOHN 
< — -a.  -in  » 

ina.ujin 

^ICOO-'U.-' 
XQ-  ► *>»H^U_CQ 
X — . — 


O0 

— •*  «*o— x-* 

LU 

m *.>r 

CO 

•"T  — — > — 

O 

^zi-aQom 

Q- 

— atx<  «*--4x 

CO 

X «jOx~*  «-n 

0 

m 

z 

X — CLZO-  — 

0 

H 

< 

a.m  *-<r~<om<\j 

0 

•k 

□c 

- — O in  

•— • 

H 

h- 

h- 

— inm  •~ino.  - 

00 

II 

m — - — t—  — x ~o 

► 

-5 

1 — 

X 

> • 

•* 

LU 

ST  — 

inar—wix— .0 

0 

«•>» 

0 

<h 

— 

-r-.-caNo; 

O no 

“5 

1 

(MH 

,1-tI  Q-fvlXI— — O 

O — • 

► 

CO 



1 1 

JO  - .<  -f--> 

**+  v. 

2T 

LU 

zjla 

CL 

men 

X ■ — — —ox 

m 0 

— * 

— » 

*> 

O 

G 

1—^  N4 

x— mm— 0 „ « 

► in 

0 

z 

z 

zwi’zzz 

c 

UJ  * * 

a.in—~o>-<— — 

0 ^ 

CL 

► 

•* 

CL 

-J 

z mm 

► ^-iinrvj 

UJ  0 -4- 

Q 

H 

1- 

Z -2<<Z 

— • in  Ox  in — — 

X < 

z 

3" 

< 

-O0>z?-  - 

h- 

I—  acco 

in — cc  ~>^iu 

— O—  • 

X *»4 

X 

y 

y 

"lOJJ- 

0 

-«X-kXun 

1—  — (\im 

w • 

X 

V 

x **o <<X 

-J 

s-*3 

Hi  ►in^u;  «.  - 

-5  CO  •*»  >0 

1 — vO 

X 

Q. 

h- 

Q-a:— oo>-^ 

CO 

4-  .-4  *0  0 

ro- xi 

— ■ '•vO  ►OLL 

w 

■*— » 

LU  • 1 HDCa 

x ini— iO  «(\|0 

0 — »_jii-  o.-im  cu 

0 

0 

< 

OOQOcoun 

LU 

z 0 x 11  c 0 

XZ  -OX. 

omomo  11 

<* 

< 

<<!<<ZZ 

2: 

*h  < 0 Xm>> 

xoin  » -omm 

1 

UJ 

LU 

i/l 

UJLULUlU  < <t 

*•4 

►-OO  -XI 

1 »■  *10w 

00 

H 

a: 

CL 

z 

atixotacara: 

1— 

— lOoiO  11  11 

2:coa-inoin  ..in 

rnm(_in<  11  a> 

— < 

1 

> 

1 

z 

a 

JJJIhi- 

II  0 1 O'  0— — 

oZl-H— U_ 

X 

X 

O 

UJ 

ujoi-  omm 

3-uJUJXinxrgu. 

-|-hhGJQo:ON<  cl 

O 

—1 

0 

_J  j i — J J — J 

X 

z mxovo— — 

11  11  11  <a:<cO 

LUO 

w 

-J 

w 

-J 

_i 1— •— J — » — ' 

— 11  11  0 

c — xzinoc'i'a  11 

11  i^cooiuouJii-^Ca:^ 

X 

<T 

LL 

<1 

<<!<!<<< 

►-»-  ^ LJCOCD 

uOaOxhUXznMHau.a:  ^C; 

0 

O 

oooooo 

►— 

— xm  ox>- 

i,  i»  <1 

0 

n 

O 

CL 

H 

O' O' 

<1 

< 

< 

< 

0 0 

O'er* 

c 

0 

LU 

H- 

O <M 

n-N- 

-J 

-J 

CL 

CO 

0 

O 

O 

uoo 

80 


THEFIV  (con't) 


OOOOOOOOOOOOOOOOOOOOOO 
fM  cn  m r- oo  O' o ^ um  sj- it  cc  0 O cvi 

r~f~.r~r-f^(^r-aic»0'0'(j'0'0'0'  O'O'O'O'OOO 
OO  OOOOO  OOOO 00000000—.-i,-i 
OOOOOOOOOOOOOOOOOOOOOO 

LU  UJ  LU  UJ  UJ LU  UJ  UJ IXJ  LL)  UJ  UJ  UJ  UJ  UJ  UJ 1X1 UJ  UJ  UJ  UJ  QJ 


OOOOO 

pl^lT'ON 

OOOOO 

OOOOO 

ujujujujuj 

irnr 


OOOOO OOOO 
o f\j  ro  vj-  lt\  c r — cc 

-*  «-)  —I  M *-•«-> 

)»-) 

OOOOO OOOO 
UjUJuJUJUJUJUUJUJ 

nninii 


ooooooo 

oj  ( o 

CV4CMCM^<M<N<NJ 

f*3 

ooooooo 

UjLULUUJl^-IUJll 


X 

OJ 


LU 

X 


u_ 

o 


h- 

INI 

UJh- 

CL 

r— 

VU 

“5 

< 

Zd 

•> 

<c 

— CL 

O 

•— • 

a — , 

: z ' 

CL>- 

-5^ 

> - 

o 

fc— * 

OX 

OQ 

<r 

X 

Of- 

QC  cl 

a: 

CL 

•»  •» 

— - 

X 4* 

►— 

— 

ujo 

X — 

1— t 

♦ • 

X 

— » 

h-H- 

OCL 

Ql 

to 

»— « 

** 

< 1 

-ZI 

— II 

h— 

OC 

XH- 

COOuj 

1 

a.— 

a: 

1— 

— 

— Z 

rvj  *f- 

— Q> 

< 

< 

Nl 

H-UJ 

*20  X 

ac 

— n 

i— 

r 

+ 

to  2: 

CO  »o  o 

O 

*— • •» 

o 

tO 

— * 

•JJUJ 

>o  * X 

CL 

oa 

H 

s 

x 

m 

+CL 

“ »2  UJ 

CL 

♦ — . 

•* 

o 

w 

zo 

(03  * O 

Zuj 

•*«» 

nl 

•— • 

— 4tO 

X O X — O 

r 

7-4  X 

c 

— 

K 

in! 

<<l 

«JtO—  X 

III 

I x 

h- 

►-* 

4- 

LJUJ 

3IZ  * — 

JO 

wO 

— * 

X 

to 

X 

o *o>—  X 

O 

7^0. 

me 

C\J 

z 

Csi 

LU 

OCr-4  »— • 

OLU 

. 

«*• 

< 

w — 

— 

H-UJ 

IlldZ- 

o 

* — 

• 

lO 

tO 

CL 

<r 

*^o+  -X 

2:z 

2 II  “>-> 

m:s 

LU 

“5 

H- 

r%J  |—^{\JC\| 

— j • 

(/)—•  idcr'  Cl  O 

•*o_  •*  • 

x< 

3l 

•» 

4-  CNJ  1 C\J  •* 

a— — — 

o>- 

-5— O-XX  II  2 

<►- 

HlO-O 

GO  • 

►a: 

*-* 

■— » 

LU 

— (\J 

t—  H(M)<1 

o 

Md—UJX  J 

Ooc 

II  — UJUJ 

aO(X 

K- 

rvi 

h- 

r*  II  1— 

■ t ' ■ | , . 

— J LU 

H3l-l-t--H  II 

< 

Q-O"-"  *OUJ 

wO 

— * 

< 

OOOO 

<z 

II0-I_|0  — 

>-> 

— <r~XazO 

o • 

— * — 

LU 

Q 

h- 

*— • 1“  1 Mil  — 

-XI — 1— ►— 

uo 

-»ccjjo<ix 

ton 

0>-  — 2 

OL  — 

O'  +•  • 

x 

< 

tO 

N-»oiad  c\j 

UJ  II  II  II 

1 

IX  X ii  -I*- 

aco 

r^(~o~K  — 

0~< 

mxoo 

a: 

UJ 

ii  ii  •. 

CC  — — 

3? 

r~oX>x  ^ 

I'OO-U.Zt- 

r-r^oo 

h- 

ac 

LU 

* • II  ||  — ~4 

□£  X X X 

oo 

O'  oo— 

LL  UJ 

"02 

IH 

••wOO 

Q 

X 

w 

CL 

JJJ7DO 

X 

o oo 

II  sO  O 

LU 

h- 

UNLUm^  •*  •*  — • 

■ — • — ■. . 

IQ 

n_i— » iuj 

LUK 

o o 

~x  +• 

— < 1 1 

X 

-J 

rg  • •.cm^csJX 

_JX>lxl 

UJ 

Q<<<Q 

X 

ro^  0001X31— 

1— 

-J 

> 

• *:^m— -i-a 

_lOOO 

h-iy'1 

aouO 

H-LU 

~*X  — 

4— OCtOX 

< 

CL 

||  ,| 

<aloc& 

UJ< 

I— 

ww  II 

-*o  ll  II 

UJ 

O 

< 

II 

Ot-I-K 

OCC 

O0<T 

03 OCT) CCO.  31— 

H- 

> 

I'lllllll-H 

i 

rM 

2 tOX 

X 

o.a.ao.  < 

H-CL 

X 

CL 

LU 

ton 

too 

T 

2F 

OCCL 

— — J 

O 

O' 

a 

O 

H 

x< 

o 

m 

o 

r— 4 

1— 

LULU 

*H  O 

f-o 

O'M 

>o 

o 

h-  *-4 

r-M 

o<jo  uuu 

OO 

oo 

81 


THEFIV  (con't) 


oo  ooo 
oo  <t  or~m 
f\icn  rryrrisj- 

i l^l»  j 

oo  ooo 

LUO)  LUUJUJ 
XX  III 
H-H-  »—*—•— 


oooooo  o 
-oh-qco'  0,1  <r 
vj-u~un  in 

oooooo  o 

LUUUUJUJujUJ  lu 

mni  x 

l— »— ♦-  H- 


OOOOOO  OOOOOOOOOOOOO 
or^-ccu'O^i  roj-ir\cDO'o^t'ic'i^mcc'oN 

inminmovo  o-oococo  0'0'0'0'0'U'Q'0' 

OOOOOO  OOOOOOOOOOOOO 
LUUJUJ  LUUJUJ  UJLUIUUJLUUJUJUJUJUJ(J:UJIJJ 

iimi  iiiiiinrmi 

H-  H-  *—  H-  H-l- 1— I— I— H-|— t—t— H->— *— H- 


OOOOOOO 
<f-in»Of— OOO'O 
OOOOOOw 
CsJ<M(MCM<\JCMpvj 
OOOOOOO 
LU  LU  LU  LU  UJ  UJ  tu 

XIIIIH 


-j- 

h- 

LULU 

*-* 

z 

I 

«• 

UJ 

a 

•• 

X 

►o 

UJ 

UJ 

LL 

—T 

r-l(t1VT\ 

X 

a 

X 

O 

XXX 

*— i 

o 

OC  II  O 

H— * 

CL  a CL 

I— 

C/*)  LL 

UJ.-HJJ 

— 

11  11  II 

< LL 

H-*  1 — 

*— • 

CJ 

a a a 

a 

LU  *•+ 

U-2^< 

CL 

•— • 

LU 

x o 

— 

<^1Q 

•* 

o 

— » 

o 

r*J 

a 

xa 

z 

LU 

CD 

inmm 

UJ 

UJ  * 

“5 

O o 

•» 

a£ 

Pvj 

xxx 

LU 

z — 

•* 

UJZ 

CL 

» 

cl  aa 

O 

O i~u 

a 

H-UJ^4 

•» 

CD 

* • • 

X 

*— 4 

— < 

<xx 

►— 

>- 

UjUJUJ 

UJ 

z — 

— 

Ov-^t; 

•» 

z 

OOo 

o 

(T 

a ^ 

— » 

LU 

CD 

• • • 

— o 

2- 

o 

a 

O -X  — 

O 

X 

X 

X 

~*ctoin 

uurn 

LU 

a 4* 

ii 

—•  a 

X 

UJ 

•» 

XXX 

~b- 

UJ 

LU  — 

— 

►Xa.  11 

XL 

CL 

t- 

a a a 

—<tc 

CD 

CO  (Vj 

•o  o zo 

r-«  UJ  X 

Q 

Cl 

■D 

< 

WWW 

-Oh 

< H-« 

in  *-•  -“5 

r H-i- 

■—* 

•» 

C/0 

X 

• • • 

H-*a 

L/> 

CQZ— 

r*  H ► 

U.Q- 

X 

< 

rN 

OOq 

► +c 

< 

■ 4 4 

CZm  ^ ll  o 

X < 

4 

O 

LU 

zzz 

*— *UJ  O 

x 

LU^-3^ 

1—  -X^Ow 

XQ  O 

— ZQ- 

o 

X 

•—*<*' 

<<< 

-^1 

h-  ii  x 

ll  ll  -J^UJ 

uj  *z 

O II 

CL 

— X 

— • • • 

ac<iD 

LU 

O II  — O r-o 

00—12:  < 

<C-  1-4 

w> 

»- 

fON 

-OK 

h- 

X -X 

OOO— '®X  z 

<—!*-* 

ll  o 

C/0 

t-  i 

oOa_  <SxL<f 

— * II 

• — ■•—«»—<  --I  v— <t_u 

i<<r^ 

-4—<0-5 

a 

OC 

<o 

*■  ••  • cO'— 'O^OIT 

H-r — 

<t  —CO  XLt-ZL 

oox 

II  X“>  •- 

o 

x-> 

L-^rotna  “-a  o<»-  • 

— • ^ 

• HNOazz 

<1  II 

O II  QLU 

QC 

u. 

rvlhj 

• • *0 

-(Lh  , 

r-3 

UJ 

Oox  O o— 

^-lO  X 

CO  Z) 

CL 

O II 

«LUUJUIOOH-  O' —JO-  II  O 

O'- 

LU^i^O  OH- 

! ZZX 

om-z 

UJ 

LL 

aQ-ixoOo^iiw  • 

moo  . 

LU  O— 4 

• xo  z 

\<!OX 

h-  ^ — • 

-J 

X 

-Hi. 

LUw^w.  • • ,XO*U_ 

— _io 

X X 

«CX  o 

o — 

in — C XI— 

h- 

-Jl-M 

1—  voinco.HrriincL  i/>  »u-^j — II 

1— 

— o o 

wK-f— h- 

XOCL2 

< 

<o 

<cQLO<nxXX  II 

11  cO—« 

<M— 

u. 

xz<z 

ox  a 

o 

LU 

ON 

c_D<i<t<a  aaujuj  ii  <-Jiu<oo 

UJ 

I-H 

ujauj 

OX  o 

H- 

II  II  II  — H* LU M h“ 3"  in  -J 

1 — 

— J X Uj  X 

< 

UJ^-irci10-- —■<  

—•a  — 

< 

<UJH-UJ 

I xxxu_u-u.  00-dU.otQO— • 

-J 

ZtL-^QC 

o 

I—  d^CL'-'^Ma.  CdO*-*  JU.QO 

o 

-.=>  o 

O 

o 

05uo—'lo 

-J 

1— 

-J 

(NJ 

•—4  a < 

< 

CM 

LU 

-H  <NI 

<o 

iH 

CXIUXUJ 

o 

tvj 

t/1 

o O 

oor- 

o cocnr-- 

OXH-X 

vO 

in  in 

LT\*-H 

— 1 HN» 

in  mm 

o 

o 

o 

OOOo 

82 


THEFIV  (con't) 


OOOO  ooooo  OOOOOOOOOOOOQOOOOOOOOOOOOOOOOOOOOOOO 

inj^imcm  (\JrorOforo  rr»i'ONj-vi-vr>4-'t'J'>4->i-u'urururMrar\ir\u'\u''in'0'£)^3'OvOvO'0'Os3'Or~f^r-r^r'r- 

rvl^fSJCvj  (Mf\|lM<N<\l  pu(VI(\J(NI<\l<\((NJ(MCvi(NifNJ(M<M(M<'lCMC  I <M  <NI  (\|<\|  0 J<\ji\J(\l(\l  CM  <M<XI  (SI  CM  C\l<\l(\|(\l  CM 

OOOO  OOOOU  OOOOOOOOOOOOOOO  OOOO OOOOO OOOO OOOO OOOO 

UJ(2JU-J  UJ  LU  LULL)  LULU  LUUJLUqjlL  LULUUULU  LULL’ LU  LULU  LUy^' LULU  LULU  LULUlijLL.  lulu  U.)  LUUJ  LULU  UjLU  LULL1  LU 

iiii  inn  iiiiiiiiiiiriiiiiiiiiiiiiiixniiiiii 

►—  H-H*  h^hhh  HHhhhh^hhl-hhKhl"HhKhhhh,H-hhhhh*"hHhH’hhh 


— 

r- ) Q- 


<t  X 

*-* 

-> 

*.  LU 

* 

u— K 

u. 

om:x 

o . 

H 

Oj^CL 

f-4 

— * 

— 

•>  “ 

x 

•— »Q_  — 

5: 

vJ 

i— 

UJ  - 

x 

X 

X— 

T 

i; 

-X  -X 

* 

X 

o 

CL 

>n.zx 

X 

x 

— » 

o 

I—  — • 

»— * •>  ► ^ 

X 

— > 

X 

►— * *k 

Hi’iin 

CL 

T72a 

CL 

•» 

LU  w 

*>  _ 

o 

» 

o 

•» 

XI- 

o •» 

x** 

LU 

U. 

•» 

•— * 

— • 

UJ  • 

oo  *••— « •* 

z 

•— * 

1-0 

“5 

> 

XXX 

LT 

►ZX7 

I/'* 

•• 

W 

UJ  —-J 

"J 

CL  *-i 

i i i 

XT 

(_)  »0-  •* 

z 

— X 

X Hi. 

•» 

^ • «— « 

o 

ox  *-o 

o 

*: 

O X 

-<  -1  II 

LU^* 

h-  •“ 

*x* 

•— < 

<x*  - 

X 

— . CL 

I-  H — 

x — 

zn-  oj 

*x* 

►— 

oa  ^Q- 

h- 

► 

w II 

U.LU 

1— « * 

On—  ► 

L-i 

— ►a.x 

o 

X z — 

UJ 

O *X  4- 

X>«*u 

M 

r zx  «n 

T"  II  — 

*-X 

X— X cj 

03  0 

o 

►III- 

O 

CL 

kXZr-kO 

~>X:*:x  ii  — cal— 

<—  Q.  k 

CCCX-CL 

UJ 

^cl  q.^ 

LU 

(1  •>  (1  » 

h^^^HZZ 

^ — - rO 

h-H-  | — 

QL 

X^'-'CL 

ad 

F— 

II  — 

• 

II  0.00.  O — o 

M ||  II 

CL 

-QQ- 

CL 

— IOX  II  n— 

o 

O ^ CT  »-j 

X II  Oh-OX"  o— 

oooo 

O' 

— *X»-*  *iu  • • 

• 

o 

o^— — — cmi-c*;  ii 

* 

•OoorowOa 

XXX 

UJ 

zctaco 

UJ 

X 

►— O'XO  OO 

o 

o 

o ii  Q.aa.O'ZO<“* 

X -O  _l  MM.  CM- 

XXX 

X 

«-»CLCL< 

X 

Xr-(0  n^ZuiO'H 

•in 

. O ^ X X x 3 — 

ox— 

o.  .a— — HO 

— w — • 

►— 

U_ 

*- 

4-*hoo  — -i^NcnOmO^OH-i:  ti  n h no 

"#■' — U i 

•— < rO  >-0 

ojJj 

-J 

-ClOrtl-Z  II  H 

II  II 

H ii 

II  II  • —OX  CO 

CNJOH*  _JLU< 

LLLLU. 

UJ 

_i— i_i 

UJ 

Joa  Qaz" 

■ h X x X r>-\' 

m LU  >-~m 

-ll-XX  UJ 

ULU-LL 

h— 

_!<<< 

H- 

<0X0  c\i*^r'>cc ~j 

<1. 

JOOU 

< 

o 

W»  «W  WWW 

ii  oa  juorcO'- ._j 

COO 

-J 

<1 

-J 

O^rjrnrO'T'riauAu  O'"* 

Tll  ll  OL<r 

Xxx 

o 

o 

o 

o'-’ooooooooxxx  an 

lu 

o 

o 

_J 

-J 

< 

< 

»o 

o 

®ao 

o 

o 

rj'COO' 

^ (\J 

•4- 

nOn 

st  rOfOO 

o o ^ 

(\J 

lX|vDcr> 

o 

o 

83 


OOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOO 
r- co  o' o oj  m oo  O' o ,-1  --O  f*- co  O' o <\j  o>  .4- u ■>  so  r- 

r~  f*.  (v.  [>-  CD  <D  CD  CD  CD  CO  CO  00  00  CO  O'  O'  O'  O'  O' O'  O'  O'  O'  O'  O O O O O <J  o o 


000 0000 0000 00000 000 00 000 OOO OOOOO 

LLKUUJUJLUUJLUUJ(ijUJujUJUJaJUJUJUjUJlUU’UJUJLUUJu.iaJUJLUUJUJLUUJ 

IXXlIIIXlIlIXXIXlXIIXlIIIItXIIXl 


p 

: 


— » 

X 

— • 

> 

o 

O 

— > 

— 

•> 

UJ 

•» 

UJ 

— 

— 

fO 

UJ 

3 

m 

UJ 

3 

m 

X 

•> 

2. 

a: 

— » 

X 

CL 

•• 

— * 

(MLU 

■— 

■— t 

►— 

CNJUJ 

•— « 

k- 

(\JUJ 

• 

m 

cO 

F3T 

»— 

•r 

— 

k- 

-r 

UJ 

► 

> 

aji— . 

UJ 

T 

UJ  •— « 

UJ 

3” 

LU  ►— « 

3" 

UJ 

r>- 

X 

O 

rF 

•» 

O 

T>- 

►— « 

— z 

>- 

H 

it: 

*— * — » 

— • 

fO 

a: 

•— < 

F- 

CSjN^ 

h-  •* 

k- 

a. 

CL 

k-  •» 

»- 

U. 

CL 

h-  •* 

~*x 

LL 

u. 

00 

LLI  *— * 

LJ 

to 

•— » 

Z 

— o 

oO 

*— « 

Z 

> 

-I  - 

> 

Q 

O 

> 

Q 

o 

^IfUX 

X 

XH 

X 

—4 

•XF 

X 

-XF 

X 

-F-3 

a 

•»  •» 

h- 

k~ 

(M  * * 

k- 

•■ 

►— 

h- 

Csl~CL 

k-»— 

►- 

< 

>—• 

h* 

< 

CL 

•*  *4— 

cc 

CO 

z 

•— • 

^zz 

oo 

Z 

hZZ 

UJ 

mm  - 

O 

3^> 

o 

3 

> 

0=53 

C 

3 

> 

uDn 

O 

ox  x 

O 

do 

a 

o 

UJ 

•OO 

CL 

O 

UJ 

•OO 

•* 

UJ 

X 

a 

nwac 

> 

Q 

rvj 

^•HCL 

O 

<x<r-*-  — <i  — 

. <v  — ■ 

o<t<— 

■ • 

>ox» 

-•»  — * 

t •O'— ’if*  *o 

w — w<NJ  -o 

1 . 

U3  *0 

— w — fO  •Ow— 

’ **o 

aa.oxoa.oxoaa.aoxoacxoaaiOxoana.oxQ 
t— mmOit— oomc'h-t— »— como't— aoino'*— h-k  aoino'f-t— ®mO' 
OO  •xm  «0  -nj  hOOO  **\)  »-0  *<\»  .aoo  *'<NJ  -oco  -n.i  «■ 

-O—iO — -o  j-Jod—'O 

'u-Q |-«a>! 0.^0 I I 

Ui«tuj  lu^uj  uj<Iai  llklli  uu<tuj  uj<uu 

JJKII-- <►— 3T  H-  — I — I -J(-X  ►-_!!-  T 1 

_l  -J  CC  -J  — > ct  -x  _l  — / ~J — CL  M -J  —1  CC  — < -I  —I  — I oc  — _l  —1  -I  <x  — . a 
< < a lj  or  **  of  o ac  «i  < a:  □ oc  < <x  o cc  < <r  < <y  o ce  < <t<  ct  O a.  2" 
ouJu.xoiu.juuuju.juJaJoooJu.Juooju.iai 


in 

' u 

o 

04 

a 

m 

4- 

o 

ao 

O 

00 

o 

o 

00 

ao 

oo 

CD 

34 


APPENDIX  C 

TRADITIONAL  EXTENDED  KALMAN  FILTER 
THESIS 
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